#include "mainwindow.h"
#include "ui_mainwindow.h"
#include <QRandomGenerator>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QWidget>
#include <QLabel>
#include <QPushButton>
#include <QApplication>
#include <QString>
#include <QDebug>
hduVector3Dd position;
hduVector3Dd force = { 0, 0, 0 };
hduVector3Dd JointEncoders;
hduVector3Dd GimbalEncoders;
hduVector3Dd anchor;
HDboolean bRenderForce = HD_FALSE;
MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent)
    , ui(new Ui::MainWindow)
    , plotX(new QCustomPlot(this))
    , plotY(new QCustomPlot(this))
    , plotZ(new QCustomPlot(this))
    , timer(new QTimer(this))
{
    ui->setupUi(this);
    // 设置窗口标题
    setWindowTitle("主从控制系统实时力值与坐标显示");

    // 创建中心部件
    QWidget *centralWidget = new QWidget(this);
    setCentralWidget(centralWidget);

    // 创建垂直布局用于整体布局
    QVBoxLayout *mainLayout = new QVBoxLayout(centralWidget);

    // 创建水平布局用于放置三个图像
    QHBoxLayout *topLayout = new QHBoxLayout();

    // 设置每个绘图的大小策略，使其尽可能保持正方形
    plotX->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred);
    plotX->setMinimumSize(200, 200);
    plotY->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred);
    plotY->setMinimumSize(200, 200);
    plotZ->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Preferred);
    plotZ->setMinimumSize(200, 200);

    topLayout->addWidget(plotX);
    topLayout->addWidget(plotY);
    topLayout->addWidget(plotZ);

    // 创建一个 QWidget 作为上半部分布局的容器
    QWidget *topWidget = new QWidget(this);
    topWidget->setLayout(topLayout);

    // 限制上半部分的最大高度，这里设置为 600 像素，可根据需求调整
    topWidget->setMaximumHeight(600);

    // 将上半部分的容器添加到主垂直布局
    mainLayout->addWidget(topWidget);

    // 创建一个新的垂直布局用于放置下半部分的控件
    QVBoxLayout *bottomMainLayout = new QVBoxLayout();

    // 创建水平布局用于放置力值标签
    QHBoxLayout *forceLayout = new QHBoxLayout();
    font.setPointSize(14);  // 点大小（更适合跨平台显示）
    // 创建三个 QLabel 显示力的值
    labelX = new QLabel("X方向力:0", this);
    labelY = new QLabel("Y方向力:0", this);
    labelZ = new QLabel("Z方向力:0", this);
    labelX->setFont(font);
    labelY->setFont(font);
    labelZ->setFont(font);
    forceLayout->addWidget(labelX);
    forceLayout->addWidget(labelY);
    forceLayout->addWidget(labelZ);

    // 创建水平布局用于放置坐标值标签
    QHBoxLayout *coordLayout = new QHBoxLayout();

    // 创建三个 QLabel 显示坐标值
    coordXLabel = new QLabel("X 坐标: 0", this);
    coordYLabel = new QLabel("Y 坐标: 0", this);
    coordZLabel = new QLabel("Z 坐标: 0", this);
    // 应用到所有标签
    coordXLabel->setFont(font);
    coordYLabel->setFont(font);
    coordZLabel->setFont(font);

    coordLayout->addWidget(coordXLabel);
    coordLayout->addWidget(coordYLabel);
    coordLayout->addWidget(coordZLabel);

    // 创建一个 QPushButton
    QPushButton *button = new QPushButton("Quit", this);
    // 或者分别设定最小和最大尺寸
    //button->setMinimumSize(150, 40);  // 最小宽度为150像素，最小高度为40像素
    //button->setMaximumSize(600, 80);  // 最大宽度为300像素，最大高度为80像素

    button->setFont(font);
    button->setMinimumHeight(60);
    // 设置按钮样式：圆角、蓝色背景、白色字体
    button->setStyleSheet("QPushButton {"
                          "  background-color: blue;"
                          "  color: white;"
                          "  border-radius: 12px;"
                          "}"
                          "QPushButton:hover {"
                          "  background-color: darkblue;"
                          "}");
    // 将力值布局和坐标值布局添加到下半部分的垂直布局
    bottomMainLayout->addLayout(forceLayout);
    bottomMainLayout->addLayout(coordLayout);
    bottomMainLayout->addWidget(button);

    // 创建一个 QWidget 作为下半部分布局的容器
    QWidget *bottomWidget = new QWidget(this);
    bottomWidget->setLayout(bottomMainLayout);

    // 将下半部分的容器添加到主垂直布局
    mainLayout->addWidget(bottomWidget);

    // 设置拉伸系数，让上半部分和下半部分按比例分配空间
    mainLayout->setStretch(0, 2); // 上半部分占 2 份
    mainLayout->setStretch(1, 1); // 下半部分占 1 份

    // 初始化每个绘图
    plotX->addGraph();
    plotX->xAxis->setLabel("时间(s)");
    plotX->yAxis->setLabel("x Force/N");

    plotY->addGraph();
    plotY->xAxis->setLabel("时间(s)");
    plotY->yAxis->setLabel("y Force/N");

    plotZ->addGraph();
    plotZ->xAxis->setLabel("时间(s)");
    plotZ->yAxis->setLabel("z Force/N");

    startTime = QDateTime::currentDateTime();


    // 连接 timeout 信号到 updatePositionLabel 槽函数
    connect(timer, &QTimer::timeout, this, [this]() {
        updatePlot(position, JointEncoders, GimbalEncoders,force);
    });
    timer->start(100);

    // 连接按钮的 clicked 信号到 QApplication 的 quit 槽
    connect(button, &QPushButton::clicked, qApp, &QApplication::quit);
    // 初始化弹簧刚度
    gSpringStiffness = 0.1;


    HDErrorInfo error;
    // 初始化触觉设备，获取设备句柄
    hHD = hdInitDevice(HD_DEFAULT_DEVICE);
    // 检查设备初始化是否出错
    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
        // 打印错误信息到标准错误输出
        hduPrintError(stderr, &error, "Failed to initialize haptic device");
        // 在调试输出中提示用户按任意键退出
        qDebug() << "Press any key to quit.";
        return;
    }

    // 在调试输出中显示示例名称
    qDebug() << "Anchored Spring Force Example";

    // 安排异步回调函数，用于渲染力和更新位置信息
    gCallbackHandle = hdScheduleAsynchronous(
        [](void *pUserData) -> HDCallbackCode {


            HDErrorInfo error;

            HDint nCurrentButtons, nLastButtons;

            // 开始一帧的力渲染
            hdBeginFrame(hdGetCurrentDevice());

            // 获取当前设备的位置
            hdGetDoublev(HD_CURRENT_POSITION, position);
            hdGetDoublev(HD_CURRENT_JOINT_ANGLES, JointEncoders);
            hdGetDoublev(HD_CURRENT_GIMBAL_ANGLES, GimbalEncoders);
            // 获取当前按钮状态
            hdGetIntegerv(HD_CURRENT_BUTTONS, &nCurrentButtons);
            // 获取上一帧按钮状态
            hdGetIntegerv(HD_LAST_BUTTONS, &nLastButtons);



            // 标记开始渲染力
            // 检测按钮按下事件
            if ((nCurrentButtons & HD_DEVICE_BUTTON_1) != 0 &&
                (nLastButtons & HD_DEVICE_BUTTON_1) == 0)
            {
                // 记录锚点位置
                memcpy(anchor, position, sizeof(hduVector3Dd));
                // 标记开始渲染力
                bRenderForce = HD_TRUE;
            }
            // 检测按钮释放事件
            else if ((nCurrentButtons & HD_DEVICE_BUTTON_1) == 0 &&
                     (nLastButtons & HD_DEVICE_BUTTON_1) != 0)
            {
                // 标记停止渲染力
                bRenderForce = HD_FALSE;
                // 发送零力到设备
                force = { 0, 0, 0 };
                hdSetDoublev(HD_CURRENT_FORCE, force);
            }

            // 如果需要渲染力
            if (bRenderForce)
            {
                // 计算弹簧力 F = k * (anchor - pos)
                hduVecSubtract(force, anchor, position);
                // 缩放力向量，乘以当前弹簧刚度
                hduVecScaleInPlace(force, static_cast<MainWindow*>(pUserData)->gSpringStiffness);
                // 设置当前力
                hdSetDoublev(HD_CURRENT_FORCE, force);
            }


            // 结束一帧的力渲染
            hdEndFrame(hdGetCurrentDevice());



            // 检查是否有错误发生
            if (HD_DEVICE_ERROR(error = hdGetError()))
            {
                // 如果是力渲染错误，停止渲染力
                if (hduIsForceError(&error))
                {
                    bRenderForce = HD_FALSE;
                }
                // 如果是调度器错误，结束回调
                else if (hduIsSchedulerError(&error))
                {
                    return HD_CALLBACK_DONE;
                }
            }

            // 继续执行回调
            return HD_CALLBACK_CONTINUE;
        }, this, HD_MAX_SCHEDULER_PRIORITY);

    // 启用力输出
    hdEnable(HD_FORCE_OUTPUT);
    // 获取设备支持的最大刚度
    //hdGetDoublev(HD_NOMINAL_MAX_STIFFNESS, &gMaxStiffness);
    // 启动调度器
    hdStartScheduler();
    // 检查调度器启动是否出错
    if (HD_DEVICE_ERROR(error = hdGetError()))
    {
        // 打印错误信息到标准错误输出
        hduPrintError(stderr, &error, "Failed to start scheduler");
        // 在调试输出中提示用户按任意键退出
        qDebug() << "Press any key to quit.";
        return;
    }
}

MainWindow::~MainWindow()
{
    delete ui;
    delete plotX;
    delete plotY;
    delete plotZ;
    delete timer;
    delete labelX;
    delete labelY;
    delete labelZ;
    delete coordXLabel;
    delete coordYLabel;
    delete coordZLabel;

    // 停止调度器
    hdStopScheduler();
    // 取消异步回调
    hdUnschedule(gCallbackHandle);
    // 禁用触觉设备
    hdDisableDevice(hHD);
}

void MainWindow::updatePlot(const hduVector3Dd& position, const hduVector3Dd& JointEncoders, const hduVector3Dd& GimbalEncoders,const hduVector3Dd& force)
{
    // 将弧度制转换为角度值
    hduVector3Dd roundedPosition;
    roundedPosition[0] = position[0];
    roundedPosition[1] = position[1];
    roundedPosition[2] = position[2];
    hduVector3Dd roundedforce;
    roundedforce[0] = force[0];
    roundedforce[1] = force[1];
    roundedforce[2] = force[2];
    hduVector3Dd angleJointEncoders;
    angleJointEncoders[0] = JointEncoders[0] * (180.0 / 3.1415926);
    angleJointEncoders[1] = JointEncoders[1] * (180.0 / 3.1415926);
    angleJointEncoders[2] = JointEncoders[2] * (180.0 / 3.1415926);

    hduVector3Dd angleGimbalEncoders;
    angleGimbalEncoders[0] = GimbalEncoders[0] * (180.0 / 3.1415926);
    angleGimbalEncoders[1] = GimbalEncoders[1] * (180.0 / 3.1415926);
    angleGimbalEncoders[2] = GimbalEncoders[2] * (180.0 / 3.1415926);


    QDateTime currentTime = QDateTime::currentDateTime();
    double elapsedSeconds = startTime.msecsTo(currentTime) / 1000.0;


    for (int i = 0; i < 3; ++i) {
        // 利用qRound函数实现四舍五入并保留两位小数
        roundedforce[i] = qRound(roundedforce[i] * 100) / 100.0;
    }
    times.append(elapsedSeconds);
    valuesX.append(roundedforce[0]);
    valuesY.append(roundedforce[1]);
    valuesZ.append(roundedforce[2]);

    // 移除超出显示范围的数据
    while (!times.isEmpty() && elapsedSeconds - times.first() > MAX_DISPLAY_TIME) {
        times.removeFirst();
        valuesX.removeFirst();
        valuesY.removeFirst();
        valuesZ.removeFirst();
    }
    // 获取已存在的图表（索引0）
    QCPGraph* graphX = plotX->graph(0);
    QCPGraph* graphY = plotY->graph(0);
    QCPGraph* graphZ = plotZ->graph(0);
    if(roundedforce[0]<0.3 && roundedforce[0]>-0.3)
    {
        // 设置曲线颜色为蓝色
        graphX->setPen(QPen(Qt::blue));
    }
    else
    {
        // 设置曲线颜色为红色
        graphX->setPen(QPen(Qt::red));
    }
    if(roundedforce[1]<0.3 && roundedforce[1]>-0.3)
    {
        // 设置曲线颜色为蓝色
        graphY->setPen(QPen(Qt::blue));
    }
    else
    {
        // 设置曲线颜色为红色
        graphY->setPen(QPen(Qt::red));
    }    if(roundedforce[2]<0.3 && roundedforce[2]>-0.3)
    {
        // 设置曲线颜色为蓝色
        graphZ->setPen(QPen(Qt::blue));
    }
    else
    {
        // 设置曲线颜色为红色
        graphZ->setPen(QPen(Qt::red));
    }


    // 更新数据并刷新
    graphX->setData(times, valuesX);
    plotX->xAxis->setRange(elapsedSeconds - MAX_DISPLAY_TIME, elapsedSeconds);
    plotX->yAxis->rescale();
    plotX->replot();

    graphY->setData(times, valuesY);

    plotY->xAxis->setRange(elapsedSeconds - MAX_DISPLAY_TIME, elapsedSeconds);
    plotY->yAxis->rescale();
    plotY->replot();

    graphZ->setData(times, valuesZ);
    plotZ->graph(0)->setData(times, valuesZ);
    plotZ->xAxis->setRange(elapsedSeconds - MAX_DISPLAY_TIME, elapsedSeconds);
    plotZ->yAxis->rescale();
    plotZ->replot();

    // 更新三个 QLabel 的文本内容显示力的值
    labelX->setText(QString("X force:%1").arg(roundedforce[0]));
    labelY->setText(QString("Y force:%1").arg(roundedforce[1]));
    labelZ->setText(QString("Z force:%1").arg(roundedforce[2]));

    // 更新三个 QLabel 的文本内容显示坐标值
    coordXLabel->setText(QString("X 坐标: %1").arg(roundedPosition[0], 0, 'f', 2));
    coordYLabel->setText(QString("Y 坐标: %1").arg(roundedPosition[1], 0, 'f', 2));
    coordZLabel->setText(QString("Z 坐标: %1").arg(roundedPosition[2], 0, 'f', 2));
}
